#define PI 3.1415926535

// global variable used to traverse d_rnds[] to retrive a random number.
// it's global because random numbers are needed throughout various functions
// and if it wasn't global the same random numbers would be chosen throughout
// the run
__device__ int rnd = 0;

__device__ void calcEigenvalues(double A_11, double A_12, double A_13, 
								double A_22, double A_23, double A_33, 
								double & val1, double & val2, double & val3);

__device__ void calcEigenvectors(double A_11, double A_12, double A_13, 
								 double A_22, double A_23, double A_33, 
								 double val1, double val2, double val3, 
								 point_3d_t & vec1, point_3d_t & vec2, point_3d_t & vec3);

__device__ void gauss(double (*A)[7], uint rows, uint cols, double* results) 
{
	int i,j,k,u,maxi;
	double var, t;

	i = 0;
	j = 0;
	
	while (i < rows && j < cols) 
	{
		// find the row with the maximum value
		maxi = i;
		for (k=i+1; k < rows; k++) 
		{
			if (fabs(A[k][j]) > fabs(A[maxi][j]))
			{
				maxi = k;
			} 
		}
	
		if (fabs(A[maxi][j]) != 0 ) 
		{
			// swap rows
			if (i != maxi) 
			{
				for (u=0; u < cols; u++) 
				{
					t = A[i][u];
					A[i][u] = A[maxi][u];
					A[maxi][u] = t;
				}
			}

			// reduce pivot element to 1
			var = A[i][j];
			for (k=0; k < cols; k++)
			{
				A[i][k] /= var;
			}

			// remove the pivot element from all subsequent rows
			for (u=i+1; u < rows; u++) 
			{
				var = A[u][j];
				for (k=j; k < cols; k++)
				{
					A[u][k] -= A[i][k] * var;
				}
			}
			i++;
		}
		j++;
	}

	// retrieve the results
	for (i=rows-1; i >= 0; i--) 
	{
		var = A[i][cols-1];
		for (j=cols-2; j > i; j--) 
		{
			var -= A[i][j] * results[j];
		}
		results[i] = var;
	}
}

__device__ bool matrix_transpose(double a[10][6], double at[6][10], int aC, int aR, int atR, int atC) {
	if (aC != atR)
		return false;

	if (aR != atC)
		return false;

//	printf("matrix transpose....=========================\n\n");

	for(int i=0; i < aR; i++) {
		for(int j=0; j < aC; j++) {
			at[j][i] = a[i][j];
		}
	}

	return true;
}

__device__ bool matrix_multiply(double a[6][10], double b[10][6], double c[6][6], int aC, int aR, int bC, int bR) 
{
	if (aC != bR)
		return false;

	for (int i=0; i < aR; i++) {
		for (int j=0; j < bC; j++) {
			c[i][j] = 0.0;
			for (int m=0; m < aR; m++) {
				c[i][j] += a[i][m] * b[m][j];
			}
		}
	}

	return true;
}

__device__ bool matrix_multiply2(double a[6][10], double b[10][1], double c[6][1], int aC, int aR, int bC, int bR) 
{
	if (aC != bR)
		return false;

	for (int i=0; i < aR; i++) {
		for (int j=0; j < bC; j++) {
			c[i][j] = 0.0;
			for (int m=0; m < aR; m++) {
				c[i][j] += a[i][m] * b[m][j];
			}
		}
	}

	return true;
}

__device__ void fitPoints6D(point_3d_t * pxls, double & A_11, double & A_12, double & A_13,
							  double & A_22, double & A_23, double & A_33)
{
//	printf("fitPoints6D\n");
	int i,j;

	// allocate
	double results[6];
	double aug[6][7];
	double X[10][6];
	double Xt[6][10];
	double XtX[6][6];
	double Y[10][1];
	double Xty[6][1];

	// fill the X matrix
	for (i=0; i < 10; i++) 
	{
		X[i][0] = pxls[i].x * pxls[i].x;
//		printf("X[%d][0]....%.4f\t", i, X[i][0]);
		X[i][1] = pxls[i].y * pxls[i].y;
//		printf("X[%d][1]....%.4f\t", i, X[i][1]);
		X[i][2] = pxls[i].z * pxls[i].z;
//		printf("X[%d][2]....%.4f\t", i, X[i][2]);
		X[i][3] = 2.0*(pxls[i].x * pxls[i].y);
//		printf("X[%d][3]....%.4f\t", i, X[i][3]);
		X[i][4] = 2.0*(pxls[i].x * pxls[i].z);
//		printf("X[%d][4]....%.4f\t", i, X[i][4]);
		X[i][5] = 2.0*(pxls[i].y * pxls[i].z);
//		printf("X[%d][5]....%.4f\n", i, X[i][5]);
	}

//	printf("\n\n");

	// fill the Y matrix
	for (i=0; i < 10; i++) {
		Y[i][0] = 1.0;
	}
	
	// create the X transpose matrix
	matrix_transpose(X,Xt,6, 10, 6, 10 );


/*	for (i=0; i < 10; i++) 
	{
		printf("Xt[%d][0]....%.4f\t", i, Xt[i][0]);
		printf("Xt[%d][1]....%.4f\t", i, Xt[i][1]);
		printf("Xt[%d][2]....%.4f\t", i, Xt[i][2]);
		printf("Xt[%d][3]....%.4f\t", i, Xt[i][3]);
		printf("Xt[%d][4]....%.4f\t", i, Xt[i][4]);
		printf("Xt[%d][5]....%.4f\n"  , i, Xt[i][5]);
	}
*/
//	printf("\n\n");
	//matrix_print("X",&X);
	//matrix_print("Xt",&Xt);

	// multiply X and Xt into the augmented matrix
	matrix_multiply(Xt,X,XtX, 10, 6, 6, 10);

	//matrix_print("XtX",&XtX);

	// multiply y by the transpose of X
	matrix_multiply2(Xt,Y,Xty, 10, 6, 1, 10);

	// copy the results into the augmented matrix
	for (i=0; i < 10; i++) {
		for(j=0; j < 6; j++)
			aug[i][j] = XtX[i][j];
		aug[i][6] = Xty[i][0];
	}

	// run a gaussian solver on the system of equations
	//printf("==========================================\n");
	//matrix_print("Augmented (pre-gauss)",&aug);
	gauss(aug,6, 7, results);
	//matrix_print("Augmented (post-gauss)",&aug);
	//printf("==========================================\n");

	// extract the results
	A_11 = results[0];
	A_22 = results[1];
	A_33 = results[2];
	A_12 = results[3];
	A_13 = results[4];
	A_23 = results[5];
	
//	printf("A_11: %f\n", A_11);
//	printf("A_22: %f\n", A_22);
//	printf("A_33: %f\n", A_33);
//	printf("A_12: %f\n", A_12);
//	printf("A_13: %f\n", A_13);
//	printf("A_23: %f\n", A_23);
}




__device__ void calcEigenvalues(double A_11, double A_12, double A_13, double A_22, double A_23, double A_33, double & val1, double & val2, double & val3)
{
//	printf("calcEigenvalues\n");

	// This definitely works...don't change.
	
	double inv3 = 1.0/3.0;
	double root3 = sqrt(3.0);
	double a00 = A_11;
	double a01 = A_12;
	double a02 = A_13;
	double a11 = A_22;
	double a12 = A_23;
	double a22 = A_33;
	double c0 = a00*a11*a22 + 2.0*a01*a02*a12 - a00*a12*a12 - a11*a02*a02 - a22*a01*a01;
	double c1 = a00*a11 - a01*a01 + a00*a22 - a02*a02 + a11*a22 - a12*a12;
	double c2 = a00 + a11 + a22;
	double c2Div3 = c2*inv3;
	double aDiv3 = (c1 - c2*c2Div3)*inv3;
	if (aDiv3 > 0.0) { aDiv3 = 0.0; }
	double mbDiv2 = 0.5*(c0 + c2Div3*(2.0*c2Div3*c2Div3 - c1));
	double q = mbDiv2*mbDiv2 + aDiv3*aDiv3*aDiv3;
	if (q > 0.0) { q = 0.0; }
	double magnitude = sqrt(-aDiv3);
	double angle = atan2(sqrt(-q),mbDiv2)*inv3;
	double cs = cos(angle);
	double sn = sin(angle);
	double temp1 = c2Div3 + 2.0*magnitude*cs;
	double temp2 = c2Div3 - magnitude*(cs + root3*sn);
	double temp3 = c2Div3 - magnitude*(cs - root3*sn);
	
	val1 = temp1;
	if(temp2 > val1)
	{
		val2 = val1;
		val1 = temp2;
	}
	else
	{
		val2 = temp2;
	}
	if(temp3 > val1)
	{
		val3 = val2;
		val2 = val1;
		val1 = temp3;
	}
	else if(temp3 > val2)
	{
		val3 = val2;
		val2 = temp3;
	}
	else
	{
		val3 = temp3;
	}
	
	// val1 = temp1;
	// val2 = temp2;
	// val3 = temp3;
}

__device__ void calcEigenvectors(double A_11, double A_12, double A_13, double A_22, double A_23, double A_33, double val1, double val2, double val3, point_3d_t & vec1, point_3d_t & vec2, point_3d_t & vec3)
{
//	printf("calcEigenvectors\n");
	int i,j, num;
	double magnitude = 0;
	int index = (blockIdx.x * blockDim.x + threadIdx.x); 
	rnd = index * 10; 	
	// allocate
	double results[3];
	double aug[3][4];	

	aug[0][0] = A_11 - val1;
	aug[0][1] = A_12;
	aug[0][2] = A_13;
	aug[0][3] = 0.0;
	aug[1][0] = A_12;
	aug[1][1] = A_22 - val1;
	aug[1][2] = A_23;
	aug[1][3] = 0.0;
	aug[2][0] = A_13;
	aug[2][1] = A_23;
	aug[2][2] = A_33 - val1;
	aug[2][3] = 0.0;

	// run a gaussian solver on the system of equations
//	 printf("==========================================\n");
	// matrix_print("Augmented (pre-gauss)",&aug);
	gauss(aug,3, 4, results);
	// matrix_print("Augmented (post-gauss)",&aug);
//	 printf("==========================================\n");

	do
	{
		num = (d_rnds[rnd++] % 21) + -10;
	}while(num == 0);
	// just take previously generated random number and % it to
	// a new random number between -10 and 10
	results[0] = (double)-1;
	results[1] = aug[1][2] * results[0] * -1.0;
	results[2] = (aug[0][2] * results[0] * -1.0) + (aug[0][1] * results[1] * -1.0);

//	printf("results .................... 1\n");
//	printf("results[0] .... %.8f", results[0]);
//	printf("results[1] .... %.8f", results[1]);
//	printf("results[2] .... %.8f\n", results[2]);

	
	if(results[2] < 0 && results[1] < 0)
	{
		results[0] = -results[0];
		results[1] = -results[1];
		results[2] = -results[2];
	}
	
	
	// extract the results
	magnitude = sqrt(results[0]*results[0] + results[1]*results[1] + results[2]*results[2]);
	vec1.z = results[0]/magnitude;
	vec1.y = results[1]/magnitude;
	vec1.x = results[2]/magnitude;
	
	
	aug[0][0] = A_11 - val2;
	aug[0][1] = A_12;
	aug[0][2] = A_13;
	aug[0][3] = 0.0;
	aug[1][0] = A_12;
	aug[1][1] = A_22 - val2;
	aug[1][2] = A_23;
	aug[1][3] = 0.0;
	aug[2][0] = A_13;
	aug[2][1] = A_23;
	aug[2][2] = A_33 - val2;
	aug[2][3] = 0.0;

	// run a gaussian solver on the system of equations
//	 printf("==========================================\n");
	 //matrix_print("Augmented (pre-gauss)",&aug);
	gauss(aug,3, 4, results);
	// matrix_print("Augmented (post-gauss)",&aug);
//	 printf("==========================================\n");
	 	
	do
	{
		num = (d_rnds[rnd++] % 21) + -10;
	}while(num == 0);
	// just take previously generated random number and % it to
	// a new random number between -10 and 10
	results[0] = (double)-1;
	results[1] = aug[1][2] * results[0] * -1.0;
	results[2] = (aug[0][2] * results[0] * -1.0) + (aug[0][1] * results[1] * -1.0);
	

//	printf("results .................... 2\n");
//	printf("results[0] .... %.8f", results[0]);
//	printf("results[1] .... %.8f", results[1]);
//	printf("results[2] .... %.8f\n", results[2]);
	if(results[2] < 0 && results[1] < 0)
	{
		results[0] = -results[0];
		results[1] = -results[1];
		results[2] = -results[2];
	}
	
	
	// extract the results
	magnitude = sqrt(results[0]*results[0] + results[1]*results[1] + results[2]*results[2]);
	vec2.z = results[0]/magnitude;
	vec2.y = results[1]/magnitude;
	vec2.x = results[2]/magnitude;
	
	
//	results = new double[3];
//	memset(results,0,sizeof(double) * 3);
	
//i	matrix_allocate(&aug, 3          , 4);

	aug[0][0] = A_11 - val3;
	aug[0][1] = A_12;
	aug[0][2] = A_13;
	aug[0][3] = 0.0;
	aug[1][0] = A_12;
	aug[1][1] = A_22 - val3;
	aug[1][2] = A_23;
	aug[1][3] = 0.0;
	aug[2][0] = A_13;
	aug[2][1] = A_23;
	aug[2][2] = A_33 - val3;
	aug[2][3] = 0.0;

	// run a gaussian solver on the system of equations
//	 printf("==========================================\n");
//	 matrix_print("Augmented (pre-gauss)",&aug);
	gauss(aug,3, 4, results);
//	 matrix_print("Augmented (post-gauss)",&aug);
//	 matrix_print("Augmented (post-gauss)",&aug);
//	 printf("==========================================\n");

	do
	{
		num = (d_rnds[rnd++] % 21) + -10;
	}while(num == 0);
	// just take previously generated random number and % it to
	// a new random number between -10 and 10
	results[0] = (double)-1;
	results[1] = aug[1][2] * results[0] * -1.0;
	results[2] = (aug[0][2] * results[0] * -1.0) + (aug[0][1] * results[1] * -1.0);
	

//	printf("results .................... 3\n");
//	printf("results[0] .... %.8f", results[0]);
//	printf("results[1] .... %.8f", results[1]);
//	printf("results[2] .... %.8f\n", results[2]);
	if(results[2] < 0 && results[1] < 0)
	{
		results[0] = -results[0];
		results[1] = -results[1];
		results[2] = -results[2];
	}
	
	// extract the results
	magnitude = sqrt(results[0]*results[0] + results[1]*results[1] + results[2]*results[2]);
	vec3.z = results[0]/magnitude;
	vec3.y = results[1]/magnitude;
	vec3.x = results[2]/magnitude;
	
//	printf("vec1: (%f, %f, %f)\n", vec1.x, vec1.y, vec1.z);
//	printf("val1: %f\n\n", val1);
//	printf("vec2: (%f, %f, %f)\n", vec2.x, vec2.y, vec2.z);
//	printf("val2: %f\n\n", val2);
//	printf("vec3: (%f, %f, %f)\n", vec3.x, vec3.y, vec3.z);
//	printf("val3: %f\n\n", val3);	
}


__global__ void findAxisLengthsAndOrientation(centroid_list* d_finCentroids, point_list* d_finVoters, uint* d_rds, AxisOrientation_t* d_axis)
{
	edge_pixel_t voterPxls[10];
	int k = 0, w = 0, i, rand;
	double J, I, D;
	double A_11, A_12, A_13, A_22, A_23, A_33;
	double A, B, C;// alpha, beta, theta;

	d_finalCentroids = d_finCentroids;
	d_finalVoters = d_finVoters;
	d_rnds = d_rds;
	d_axisOri = d_axis;

	int index = (blockIdx.x * blockDim.x + threadIdx.x); 
	//determine the starting location for each thread to grab
	//random numbers from d_rnds[]
	rand = index * 10;

	//assign a thread to a potential centroid, multiple centroids will split
	//the load between all threads evenly
	//i = index % d_centroidLocation;
	i = 0;
//	printf("centroid location: %d\n", d_centroidLocation);
//	for(int i = 0; i < d_centroidLocation; i++)
	{
	//	if(d_finalCentroids[i].votes >= CENTROID_TRSH_LOW)
		{
			w = 0;
		//	printf("Centroid %d\n", i);
		//	printf("=========================================\n");
//			for(int j = 0; j < d_finalCentroids[i].votes * DATA_POINTS; j++)
			{
				// printf("before\nx: \t %f\n", voters[j].point.x);
				// printf("y: \t %f\n", voters[j].point.y);
				// printf("z: \t %f\n\n", voters[j].point.z);

				// translate voted points for centroid i, to make it appear
				// as if centroid i was located at the origini
//				if(d_finalVoters[j].centroidIndex == i)
				{
//					printf("before\nx: \t %f\n", d_finalVoters[j].point.x);
//					printf("y: \t %f\n", d_finalVoters[j].point.y);
//					printf("z: \t %f\n\n", d_finalVoters[j].point.z);
					
//					printf("after\nx: \t %f\n", d_finalVoters[j].point.x);
//					printf("y: \t %f\n", d_finalVoters[j].point.y);
//					printf("z: \t %f\n\n", d_finalVoters[j].point.z);
				}
			}

		//	while(w < 100)
			{
				k = 0;
				while(k < 10)
				{
					int temp = d_rnds[rand++] % d_finalCentroids[i].votes * DATA_POINTS;
					// printf("temp: %d\n", temp);
					
					if(i == d_finalVoters[temp].centroidIndex)
					{
//						printf("just spinning\n");
						voterPxls[k] = d_finalVoters[temp].point;
						k++;
					}
				}

				//for(int j = 0; j < 10; j++)
			//		printf("point #%d, x: %.8f, y: %.8f, z: %.8f\n", j, voterPxls[j].x,  voterPxls[j].y,  voterPxls[j].z);
				
				fitPoints6D(voterPxls, A_11, A_12, A_13, A_22, A_23, A_33);
				
//				printf("made it back from fits 6D\n\n");

				J = A_11*A_22 - A_12*A_12 + A_22*A_33 - A_23*A_23 + A_33*A_11 - A_13*A_13;
				
				I = A_11 + A_22 + A_33;
				
				D = A_11*A_22*A_33 + A_12*A_23*A_13 + A_13*A_12*A_23 - A_11*A_23*A_23 - A_12*A_12*A_33 - A_13*A_22*A_13;
				
				// printf("J: \t %.16f\n", J);
				// printf("I: \t %.16f\n", I);
				// printf("D: \t %.16f\n\n", D);
				
				if(J > 0.000000 && I > 0.000000 && D > 0.000000)
				{
					// We have a real ellipsoid
					// Find eigenvectors and eigenvalues
					
					double val1, val2, val3;
					point_3d_t vec1, vec2, vec3;
					
					calcEigenvalues(A_11, A_12, A_13, A_22, A_23, A_33, val1, val2, val3);
					
					calcEigenvectors(A_11, A_12, A_13, A_22, A_23, A_33, val1, val2, val3, vec1, vec2, vec3);
					
					A = 1.0 / sqrt(val1);
					B = 1.0 / sqrt(val2);
					C = 1.0 / sqrt(val3);
					
					// printf("a: %f\n", A);
					// printf("b: %f\n", B);
					// printf("c: %f\n\n", C);
					
					double temp1x,temp1y,temp1z,temp2x,temp2y,temp2z,temp3x,temp3y,temp3z;
					
					if(vec1.y < 0)
					{
						if(vec1.z < 0)
						{
							//printf("if-if \n");
							temp1x = vec1.x;
							temp1y = vec1.y;
							temp1z = vec1.z;
							
							temp2x = -vec3.x;
							temp2y = -vec3.y;
							temp2z = -vec3.z;
							
							temp3x = vec2.x;
							temp3y = vec2.y;
							temp3z = vec2.z;
						}
						else
						{
						//	printf("if-else \n");
							temp1x = vec2.x;
							temp1y = vec2.y;
							temp1z = vec2.z;
							
							temp2x = -vec3.x;
							temp2y = -vec3.y;
							temp2z = -vec3.z;
							
							temp3x = -vec1.x;
							temp3y = -vec1.y;
							temp3z = -vec1.z;
						}
					}
					else
					{
						//printf("%f \n", vec1.z);
						if(vec1.z < 0)
						{
						//	printf("else-if \n");
							temp1x = vec2.x;
							temp1y = vec2.y;
							temp1z = vec2.z;
							
							temp2x = -vec3.x;
							temp2y = -vec3.y;
							temp2z = -vec3.z;
							
							temp3x = vec1.x;
							temp3y = vec1.y;
							temp3z = vec1.z;
						}
						else
						{
						//	printf("else-else \n");
							temp1x = vec1.x;
							temp1y = vec1.y;
							temp1z = vec1.z;
							
							temp2x = vec2.x;
							temp2y = vec2.y;
							temp2z = vec2.z;
							
							temp3x = vec3.x;
							temp3y = vec3.y;
							temp3z = vec3.z;
						}
					}
										
					double alpha = atan2(-temp3y,temp3z);
					if(alpha < 0)
						alpha += PI;
					double beta = atan2(temp3x, (-temp3y*sin(alpha)) + (temp3z*cos(alpha)));
					if(beta < 0)
						beta += PI;
					double theta = atan2(((temp2y*cos(alpha)) + (temp2z*sin(alpha))),((temp1y*cos(alpha)) + (temp1z*sin(alpha))));
					if(theta < 0)
						theta += PI;
					
					alpha = alpha*180.0/PI;
					beta = beta*180.0/PI;
					theta = theta*180.0/PI;
					// printf("alpha: %f\n", alpha);
					// printf("beta: %f\n", beta);
					// printf("theta: %f\n\n", theta);

					 d_axisOri[index].a = A * 10000;
					 d_axisOri[index].b = B * 10000;
					 d_axisOri[index].c = C * 10000;
					 d_axisOri[index].alpha = alpha * 10000;
					 d_axisOri[index].beta = beta * 10000;
					 d_axisOri[index].theta = theta * 10000;
					 d_axisOri[index].centroid = i;
					 d_axisOri[index].votes = 1;
					
					// for(int i = 0; i < 10; i++)
					// {
						// for(int j = 0; j < 10; j++)
						// {
							// printf("Distance %d, %d:\t%f\n", i, j, calcDistance(voters[voterPxls[i]].point, voters[voterPxls[j]].point));
						// }
					// }
					
				}
				
				w++;
			}
		}
	}
}
